﻿/*****************************************************************************
** Includes
*****************************************************************************/

#include "RobotCmdNode.hpp"
#include "RobotArm.h"
#include "JsonUtil.h"

/*****************************************************************************
** Namespaces
*****************************************************************************/
using namespace std::chrono_literals;
using std::placeholders::_1;

/*****************************************************************************
  ** Implementation
  *****************************************************************************/

RobotCmdNode::RobotCmdNode(RobotArm *ra)
    : Node("robotarm_node", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)), m_ra(ra)
{
    subscription_ = this->create_subscription<pccmd_msg::msg::PcCmd>(
        "PcCmdMsg", 10, std::bind(&RobotCmdNode::pcCmdMsgCB, this, _1));

    m_pubJsonBa = this->create_publisher<jsonba_msg::msg::JsonBa>("JsonBa", 10);

    getParameters();
    m_ra->connect2Host(m_strIp, m_usPort);
}

RobotCmdNode::~RobotCmdNode()
{
    rclcpp::shutdown();
}

void RobotCmdNode::pcCmdMsgCB(const pccmd_msg::msg::PcCmd::SharedPtr msg) const
{
    std::string strCmdName = msg->cmdname;

    // 过滤掉不需要的命令
    if (strCmdName == "pinch" || strCmdName == "getCloud" || strCmdName == "startLaser" || strCmdName == "stopLaser")
    {
        return;
    }

    QVariantMap mapData = JsonUtil::jsonStr2map(QString::fromStdString(msg->cmdcontent));
    if (strCmdName == "holdArm")
    {
        QString strArm = mapData.value("arm").toString();
        int nUnlock = mapData.value("unlock").toInt();
        strArm == "left" ? m_ra->setLeftEnable(nUnlock) : m_ra->setRightEnable(nUnlock);
        m_ra->call_writeDuoArmDirect(mapData.value("left").toMap(), mapData.value("right").toMap());
    }
    else if (strCmdName == "armControl" || strCmdName == "armJointControl" || strCmdName=="writeDuo_camcal")
    {
        m_ra->call_writeDuoArmDirect(mapData.value("left").toMap(), mapData.value("right").toMap());
    }
    else if (strCmdName == "stop")
    {
        m_ra->setLeftEnable(0);
        m_ra->setRightEnable(0);
        m_ra->call_writeDuoArmDirect(mapData.value("left").toMap(), mapData.value("right").toMap());
    }
    else if (strCmdName == "setTrigger")
    {
        RCLCPP_INFO_STREAM(this->get_logger(), __FUNCTION__ 
        << "setTrigger called, leftTrigger: " 
        << mapData.value("left").toMap().value("trigger").toInt()
        << "  rightTrigger: " 
        << mapData.value("right").toMap().value("trigger").toInt());

        m_ra->call_writeDuoArmDirect(mapData.value("left").toMap(), mapData.value("right").toMap());
    }
    else if (strCmdName == "controlMode")
    {
        QString strArm = mapData.value("arm").toString();
        int nValue = mapData.value("contolModeValue").toInt();
        strArm == "left" ? m_ra->setLeftControlMode(nValue) : m_ra->setRightControlMode(nValue);
        m_ra->call_writeDuoArmDirect(mapData.value("left").toMap(), mapData.value("right").toMap());
    }
    else if (strCmdName == "resetArm")
    {
        QString strArm = mapData.value("arm").toString();
        int nValue = mapData.value("errorCleanValue").toInt();
        strArm == "left" ? m_ra->setLeftErrClean(nValue) : m_ra->setRightErrClean(nValue);
        m_ra->call_writeDuoArmDirect(mapData.value("left").toMap(), mapData.value("right").toMap());
    }
    else if (strCmdName == "resetArmEnd")
    {
        QString strArm = mapData.value("arm").toString();
        int nValue = mapData.value("ftModeValue").toInt();
        strArm == "left" ? m_ra->setLeftFTMode(nValue) : m_ra->setRightFTMode(nValue);
        m_ra->call_writeDuoArmDirect(mapData.value("left").toMap(), mapData.value("right").toMap());
    }
    else if (strCmdName == "setMoveSec")
    {
        QString strArm = mapData.value("arm").toString();
        int nValue = mapData.value("moveSecMode").toInt();
        strArm == "left" ? m_ra->setLeftSecMode(nValue) : m_ra->setRightSecMode(nValue);
        m_ra->call_writeDuoArmDirect(mapData.value("left").toMap(), mapData.value("right").toMap());
    }
    else if (strCmdName == "part_trigger")
    {
        qDebug() << __FUNCTION__ << "part trigger";
        int ftMode_L = mapData.value("ftMode_L").toInt();
        int ftMode_R = mapData.value("ftMode_R").toInt();
        int periodMode_L = mapData.value("periodMode_L").toInt();
        int periodMode_R = mapData.value("periodMode_R").toInt();

        m_ra->setLeftFTMode(ftMode_L);
        m_ra->setRightFTMode(ftMode_R);
        m_ra->setLeftSecMode(periodMode_L);
        m_ra->setRightSecMode(periodMode_R);
    }
    else if (strCmdName == "operate")
    {
        m_ra->call_writeDuoArmDirect(mapData.value("left").toMap(), mapData.value("right").toMap());
    }
}

void RobotCmdNode::getParameters()
{
    RCLCPP_INFO(this->get_logger(), "getParameters...");
    get_parameter_or(std::string("name"), m_strName, std::string("noname"));
    get_parameter_or(std::string("ip"), m_strIp, std::string("no"));
    get_parameter_or(std::string("port"), m_usPort, unsigned short(0));

    RCLCPP_INFO(this->get_logger(), "name: %s, ip: %s,  port: %hu", m_strName.c_str(), m_strIp.c_str(), m_usPort);
}

void RobotCmdNode::pubJsonBa(const std::string& name, unsigned int size)
{
    auto cmd_msg = jsonba_msg::msg::JsonBa();
    cmd_msg.name = name;
    cmd_msg.size = size;
    m_pubJsonBa->publish(cmd_msg);

    // RCLCPP_INFO(this->get_logger(), "RobotCmdNode SendCmd, cmdname: %s, size: %d", name.c_str(), size);
}
